
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       borad_config.c
  * @author     baiyang
  * @date       2021-10-4
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "borad_config.h"

#include <string.h>

#include <common/time/gp_time.h>
#include <mavproxy/mavproxy.h>
#include <common/console/console.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
static board_config board_cfg;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void brd_init()
{
    board_cfg._in_error_loop = false;
    brd_board_setup(&board_cfg);
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
px4_board_type brd_get_board_type()
{
#if GP_FEATURE_BOARD_DETECT
    return board_cfg.px4_configured_board;
#else
    return BOARD_TYPE_UNKNOWN;
#endif
}

void brd_throw_error(const char *err_type, const char *fmt, va_list arg)
{
    board_cfg._in_error_loop = true;
    /*
      to give the user the opportunity to connect to USB we keep
      repeating the error.  The mavlink delay callback is initialised
      before this, so the user can change parameters (and in
      particular BRD_TYPE if needed)
    */
    uint32_t last_print_ms = 0;
    while (true) {
        uint32_t now = time_millis();
        if (now - last_print_ms >= 5000) {
            last_print_ms = now;
            char printfmt[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+2];
            snprintf(printfmt, sizeof(printfmt), "%s: %s\n", err_type, fmt);
            console_printf_textv(printfmt, arg);
            snprintf(printfmt, sizeof(printfmt), "%s: %s", err_type, fmt);
            mavproxy_send_textv(MAV_SEVERITY_CRITICAL, printfmt, arg);
        }

        rt_thread_mdelay(10);
    }
}

void brd_config_error(const char *fmt, ...)
{
    va_list arg_list;
    va_start(arg_list, fmt);
    brd_throw_error("Config Error", fmt, arg_list);
    va_end(arg_list);
}

/*------------------------------------test------------------------------------*/


